#!/usr/bin/python3
# import cv2
# import rospy
# from yolo_pkg.msg import Yolo
# import time
# import numpy as np
# from queue import Queue

# #初始化节点
# rospy.init_node("facedetect", anonymous=True)
# pub = rospy.Publisher("/face_detect", Yolo, queue_size=10)
# rate = rospy.Rate(100)

# from rknnlite.api import RKNNLite
# from concurrent.futures import ThreadPoolExecutor, as_completed



# cap = cv2.VideoCapture('/dev/came1')
# # cap = cv2.VideoCapture(0)
# modelPath = r'/home/orangepi/catkin_ws/src/yolo_pkg/rknn/face2.rknn'
 
# QUANTIZE_ON = True
 
# OBJ_THRESH, NMS_THRESH, IMG_SIZE = 0.60, 0.2, 640
# CLASSES = ("1")



# #rknnpool
# def initRKNN(rknnModel="./rknnModel/yolov5s.rknn", id=0):
#     rknn_lite = RKNNLite()
#     ret = rknn_lite.load_rknn(rknnModel)
#     if ret != 0:
#         print("Load RKNN rknnModel failed")
#         exit(ret)
#     if id == 0:
#         ret = rknn_lite.init_runtime(core_mask=RKNNLite.NPU_CORE_0)
#     elif id == 1:
#         ret = rknn_lite.init_runtime(core_mask=RKNNLite.NPU_CORE_1)
#     elif id == 2:
#         ret = rknn_lite.init_runtime(core_mask=RKNNLite.NPU_CORE_2)
#     elif id == -1:
#         ret = rknn_lite.init_runtime(core_mask=RKNNLite.NPU_CORE_0_1_2)
#     else:
#         ret = rknn_lite.init_runtime()
#     if ret != 0:
#         print("Init runtime environment failed")
#         exit(ret)
#     print(rknnModel, "\t\tdone")
#     return rknn_lite
 
 
# def initRKNNs(rknnModel="./rknnModel/yolov5s.rknn", TPEs=1):
#     rknn_list = []
#     for i in range(TPEs):
#         rknn_list.append(initRKNN(rknnModel, i % 3))
#     return rknn_list
 
 
# class rknnPoolExecutor():
#     def __init__(self, rknnModel, TPEs, func):
#         self.TPEs = TPEs
#         self.queue = Queue()
#         self.rknnPool = initRKNNs(rknnModel, TPEs)
#         self.pool = ThreadPoolExecutor(max_workers=TPEs)
#         self.func = func
#         self.num = 0
 
#     def put(self, frame):
#         self.queue.put(self.pool.submit(
#             self.func, self.rknnPool[self.num % self.TPEs], frame))
#         self.num += 1
 
#     def get(self):
#         if self.queue.empty():
#             return None, False
#         temp = []
#         temp.append(self.queue.get())
#         for frame in as_completed(temp):
#             return frame.result(), True
 
#     def release(self):
#         self.pool.shutdown()
#         for rknn_lite in self.rknnPool:
#             rknn_lite.release()

 
# def sigmoid(x):
#     return 1 / (1 + np.exp(-x))
 
 
# def xywh2xyxy(x):
#     # Convert [x, y, w, h] to [x1, y1, x2, y2]
#     y = np.copy(x)
#     y[:, 0] = x[:, 0] - x[:, 2] / 2  # top left x
#     y[:, 1] = x[:, 1] - x[:, 3] / 2  # top left y
#     y[:, 2] = x[:, 0] + x[:, 2] / 2  # bottom right x
#     y[:, 3] = x[:, 1] + x[:, 3] / 2  # bottom right y
#     return y
 
 
# def process(input, mask, anchors):
 
#     anchors = [anchors[i] for i in mask]
#     grid_h, grid_w = map(int, input.shape[0:2])
 
#     box_confidence = sigmoid(input[..., 4])
#     box_confidence = np.expand_dims(box_confidence, axis=-1)
 
#     box_class_probs = sigmoid(input[..., 5:])
 
#     box_xy = sigmoid(input[..., :2])*2 - 0.5
 
#     col = np.tile(np.arange(0, grid_w), grid_w).reshape(-1, grid_w)
#     row = np.tile(np.arange(0, grid_h).reshape(-1, 1), grid_h)
#     col = col.reshape(grid_h, grid_w, 1, 1).repeat(3, axis=-2)
#     row = row.reshape(grid_h, grid_w, 1, 1).repeat(3, axis=-2)
#     grid = np.concatenate((col, row), axis=-1)
#     box_xy += grid
#     box_xy *= int(IMG_SIZE/grid_h)
 
#     box_wh = pow(sigmoid(input[..., 2:4])*2, 2)
#     box_wh = box_wh * anchors
 
#     box = np.concatenate((box_xy, box_wh), axis=-1)
 
#     return box, box_confidence, box_class_probs
 
 
# def filter_boxes(boxes, box_confidences, box_class_probs):
#     """Filter boxes with box threshold. It's a bit different with origin yolov5 post process!
#     # Arguments
#         boxes: ndarray, boxes of objects.
#         box_confidences: ndarray, confidences of objects.
#         box_class_probs: ndarray, class_probs of objects.
#     # Returns
#         boxes: ndarray, filtered boxes.
#         classes: ndarray, classes for boxes.
#         scores: ndarray, scores for boxes.
#     """
#     boxes = boxes.reshape(-1, 4)
#     box_confidences = box_confidences.reshape(-1)
#     box_class_probs = box_class_probs.reshape(-1, box_class_probs.shape[-1])
 
#     _box_pos = np.where(box_confidences >= OBJ_THRESH)
#     boxes = boxes[_box_pos]
#     box_confidences = box_confidences[_box_pos]
#     box_class_probs = box_class_probs[_box_pos]
 
#     class_max_score = np.max(box_class_probs, axis=-1)
#     classes = np.argmax(box_class_probs, axis=-1)
#     _class_pos = np.where(class_max_score >= OBJ_THRESH)
 
#     boxes = boxes[_class_pos]
#     classes = classes[_class_pos]
#     scores = (class_max_score * box_confidences)[_class_pos]
 
#     return boxes, classes, scores
 
 
# def nms_boxes(boxes, scores):
#     """Suppress non-maximal boxes.
#     # Arguments
#         boxes: ndarray, boxes of objects.
#         scores: ndarray, scores of objects.
#     # Returns
#         keep: ndarray, index of effective boxes.
#     """
#     x = boxes[:, 0]
#     y = boxes[:, 1]
#     w = boxes[:, 2] - boxes[:, 0]
#     h = boxes[:, 3] - boxes[:, 1]
 
#     areas = w * h
#     order = scores.argsort()[::-1]
 
#     keep = []
#     while order.size > 0:
#         i = order[0]
#         keep.append(i)
 
#         xx1 = np.maximum(x[i], x[order[1:]])
#         yy1 = np.maximum(y[i], y[order[1:]])
#         xx2 = np.minimum(x[i] + w[i], x[order[1:]] + w[order[1:]])
#         yy2 = np.minimum(y[i] + h[i], y[order[1:]] + h[order[1:]])
 
#         w1 = np.maximum(0.0, xx2 - xx1 + 0.00001)
#         h1 = np.maximum(0.0, yy2 - yy1 + 0.00001)
#         inter = w1 * h1
 
#         ovr = inter / (areas[i] + areas[order[1:]] - inter)
#         inds = np.where(ovr <= NMS_THRESH)[0]
#         order = order[inds + 1]
#     keep = np.array(keep)
#     return keep
 
 
# def yolov5_post_process(input_data):
#     masks = [[0, 1, 2], [3, 4, 5], [6, 7, 8]]
#     anchors = [[10, 13], [16, 30], [33, 23], [30, 61], [62, 45],
#                [59, 119], [116, 90], [156, 198], [373, 326]]
 
#     boxes, classes, scores = [], [], []
#     for input, mask in zip(input_data, masks):
#         b, c, s = process(input, mask, anchors)
#         b, c, s = filter_boxes(b, c, s)
#         boxes.append(b)
#         classes.append(c)
#         scores.append(s)
 
#     boxes = np.concatenate(boxes)
#     boxes = xywh2xyxy(boxes)
#     classes = np.concatenate(classes)
#     scores = np.concatenate(scores)
 
#     nboxes, nclasses, nscores = [], [], []
#     for c in set(classes):
#         inds = np.where(classes == c)
#         b = boxes[inds]
#         c = classes[inds]
#         s = scores[inds]
 
#         keep = nms_boxes(b, s)
 
#         nboxes.append(b[keep])
#         nclasses.append(c[keep])
#         nscores.append(s[keep])
 
#     if not nclasses and not nscores:
#         return None, None, None
 
#     boxes = np.concatenate(nboxes)
#     classes = np.concatenate(nclasses)
#     scores = np.concatenate(nscores)
 
#     return boxes, classes, scores
 
 
# def draw(image, boxes, scores, classes):
#     for box, score, cl in zip(boxes, scores, classes):
#         top, left, right, bottom = box
#         # print('class: {}, score: {}'.format(CLASSES[cl], score))
#         # print('box coordinate left,top,right,down: [{}, {}, {}, {}]'.format(top, left, right, bottom))
#         top = int(top)
#         left = int(left)
#         right = int(right)
#         bottom = int(bottom)
 
#         cv2.rectangle(image, (top, left), (right, bottom), (255, 0, 0), 2)
#         cv2.putText(image, '{0} {1:.2f}'.format(CLASSES[cl], score),
#                     (top, left - 6),
#                     cv2.FONT_HERSHEY_SIMPLEX,
#                     0.6, (0, 0, 255), 2)
 
 
# def letterbox(im, new_shape=(640, 640), color=(0, 0, 0)):
#     shape = im.shape[:2]  # current shape [height, width]
#     if isinstance(new_shape, int):
#         new_shape = (new_shape, new_shape)
 
#     r = min(new_shape[0] / shape[0], new_shape[1] / shape[1])
 
#     ratio = r, r  # width, height ratios
#     new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r))
#     dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - \
#         new_unpad[1]  # wh padding
 
#     dw /= 2  # divide padding into 2 sides
#     dh /= 2
 
#     if shape[::-1] != new_unpad:  # resize
#         im = cv2.resize(im, new_unpad, interpolation=cv2.INTER_LINEAR)
#     top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
#     left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
#     im = cv2.copyMakeBorder(im, top, bottom, left, right,
#                             cv2.BORDER_CONSTANT, value=color)  # add border
#     return im, ratio, (dw, dh)
 
# def myFunc(rknn_lite, IMG):
#     img = cv2.cvtColor(IMG, cv2.COLOR_BGR2RGB)
#     img = cv2.resize(img, (IMG_SIZE, IMG_SIZE))
#     outputs = rknn_lite.inference(inputs=[img])
 
#     input0_data = outputs[0]
#     input1_data = outputs[1]
#     input2_data = outputs[2]
 
#     input0_data = input0_data.reshape([3, -1]+list(input0_data.shape[-2:]))
#     input1_data = input1_data.reshape([3, -1]+list(input1_data.shape[-2:]))
#     input2_data = input2_data.reshape([3, -1]+list(input2_data.shape[-2:]))
 
#     input_data = list()
#     input_data.append(np.transpose(input0_data, (2, 3, 0, 1)))
#     input_data.append(np.transpose(input1_data, (2, 3, 0, 1)))
#     input_data.append(np.transpose(input2_data, (2, 3, 0, 1)))
 
#     #识别在这里
#     boxes, classes, scores = yolov5_post_process(input_data)
 
#     img_1 = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
#     if boxes is not None:
#         draw(img_1, boxes, scores, classes)
#         for i, (cls, score) in enumerate(zip(classes, scores)):
#             box = boxes[i]
#             x_min, y_min, x_max, y_max = box

#             # 计算中心坐标
#             center_x = (x_min + x_max) / 2
#             center_y = (y_min + y_max) / 2

#             msg = Yolo()
#             msg.Class = str(cls)
#             msg.x = int(center_x)
#             msg.y = int(center_y)
#             msg.Confidence = score
#             pub.publish(msg)
#     # else:
#     #     msg.Class = "NONE"
#     #     msg.x = 0
#     #     msg.y = 0
#     #     msg.Confidence = 0
            

#     return img_1



# def findring(img):
#     kernel = np.ones((3,3), dtype=np.uint8)
#     img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2BGRA)
#     img_gray = cv2.GaussianBlur(img_gray, (3,3), 0)
#     img_canny = cv2.Canny(img_gray, 64, 200)
#     img_dite = cv2.dilate(img_canny, kernel, 5)
#     #img_dite = cv2.morphologyEx(img_canny, cv2.MORPH_CLOSE, kernel, iterations=1)
#     cv2.imshow('11', img_dite)
#     # 检测圆形
#     circles = cv2.HoughCircles(img_dite, cv2.HOUGH_GRADIENT, 1, minDist=100,
#                                 param1=50, param2=40, minRadius=0, maxRadius=130)
    
#     # 如果找到了圆形
#     if circles is not None:
#         circles = np.round(circles[0, :]).astype("int")  # 转换为整数
        
#         # 找到最大圆形
#         max_circle = max(circles, key=lambda c: c[2])  # 找到半径最大的圆形
#         x, y, r = max_circle  # 提取最大圆形的参数
#         msg = Yolo()
#         msg.Class = str(2)
#         msg.x = int(x)
#         msg.y = int(y)
#         pub.publish(msg)
#         # 画圆形
#         cv2.circle(img, (x, y), r, (0, 255, 0), 4)  # 绘制圆形边界
#         cv2.circle(img, (x, y), 5, (0, 0, 255), 10)  # 绘制圆心
#         # 在圆心旁边显示坐标和半径
#         text = f"({x}, {y}), r={r}"
#         cv2.putText(img, text, (x + 10, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2)



# # 线程数
# TPEs = 6
# # 初始化rknn池
# pool = rknnPoolExecutor(
#     rknnModel=modelPath,
#     TPEs=TPEs,
#     func=myFunc)
 
# # 初始化异步所需要的帧
# if (cap.isOpened()):
#     for i in range(TPEs + 1):
#         ret, frame = cap.read()
#         if not ret:
#             cap.release()
#             del pool
#             exit(-1)
#         pool.put(frame)
# fps = 0.0
# while (not rospy.is_shutdown()):
#     ret, frame = cap.read()

#     t1 = time.time()
#     pool.put(frame)
#     frame, flag = pool.get()
#     findring(frame)
#     cv2.imshow('test', frame)

#     # fps = (fps + (1. / (time.time() - t1))) / 2
#     # print("fps= %.2f" % (fps))
#     cv2.waitKey(1)

# cap.release()
# cv2.destroyAllWindows()
# pool.release()

#!/usr/bin/python3
import cv2
import rospy
from yolo_pkg.msg import Yolo
import numpy as np
#初始化节点
rospy.init_node("facedetect", anonymous=True)
pub = rospy.Publisher("/face_detect", Yolo, queue_size=10)
rate = rospy.Rate(100)
import os
os.system("gpio mode 12 out")
os.system("gpio write 12 1")
from rknnlite.api import RKNNLite



RKNN_MODEL = r'/home/orangepi/catkin_ws/src/yolo_pkg/rknn/face2.rknn'
 
QUANTIZE_ON = True
 
OBJ_THRESH = 0.7
NMS_THRESH = 0.2
IMG_SIZE = 640
 
CLASSES = ("1")
 
 
def sigmoid(x):
    return 1 / (1 + np.exp(-x))
 
 
def xywh2xyxy(x):
    # Convert [x, y, w, h] to [x1, y1, x2, y2]
    y = np.copy(x)
    y[:, 0] = x[:, 0] - x[:, 2] / 2  # top left x
    y[:, 1] = x[:, 1] - x[:, 3] / 2  # top left y
    y[:, 2] = x[:, 0] + x[:, 2] / 2  # bottom right x
    y[:, 3] = x[:, 1] + x[:, 3] / 2  # bottom right y
    return y
 
 
def process(input, mask, anchors):
 
    anchors = [anchors[i] for i in mask]
    grid_h, grid_w = map(int, input.shape[0:2])
 
    box_confidence = sigmoid(input[..., 4])
    box_confidence = np.expand_dims(box_confidence, axis=-1)
 
    box_class_probs = sigmoid(input[..., 5:])
 
    box_xy = sigmoid(input[..., :2])*2 - 0.5
 
    col = np.tile(np.arange(0, grid_w), grid_w).reshape(-1, grid_w)
    row = np.tile(np.arange(0, grid_h).reshape(-1, 1), grid_h)
    col = col.reshape(grid_h, grid_w, 1, 1).repeat(3, axis=-2)
    row = row.reshape(grid_h, grid_w, 1, 1).repeat(3, axis=-2)
    grid = np.concatenate((col, row), axis=-1)
    box_xy += grid
    box_xy *= int(IMG_SIZE/grid_h)
 
    box_wh = pow(sigmoid(input[..., 2:4])*2, 2)
    box_wh = box_wh * anchors
 
    box = np.concatenate((box_xy, box_wh), axis=-1)
 
    return box, box_confidence, box_class_probs
 
 
def filter_boxes(boxes, box_confidences, box_class_probs):
    """Filter boxes with box threshold. It's a bit different with origin yolov5 post process!
    # Arguments
        boxes: ndarray, boxes of objects.
        box_confidences: ndarray, confidences of objects.
        box_class_probs: ndarray, class_probs of objects.
    # Returns
        boxes: ndarray, filtered boxes.
        classes: ndarray, classes for boxes.
        scores: ndarray, scores for boxes.
    """
    boxes = boxes.reshape(-1, 4)
    box_confidences = box_confidences.reshape(-1)
    box_class_probs = box_class_probs.reshape(-1, box_class_probs.shape[-1])
 
    _box_pos = np.where(box_confidences >= OBJ_THRESH)
    boxes = boxes[_box_pos]
    box_confidences = box_confidences[_box_pos]
    box_class_probs = box_class_probs[_box_pos]
 
    class_max_score = np.max(box_class_probs, axis=-1)
    classes = np.argmax(box_class_probs, axis=-1)
    _class_pos = np.where(class_max_score >= OBJ_THRESH)
 
    boxes = boxes[_class_pos]
    classes = classes[_class_pos]
    scores = (class_max_score* box_confidences)[_class_pos]
 
    return boxes, classes, scores
 
 
def nms_boxes(boxes, scores):
    """Suppress non-maximal boxes.
    # Arguments
        boxes: ndarray, boxes of objects.
        scores: ndarray, scores of objects.
    # Returns
        keep: ndarray, index of effective boxes.
    """
    x = boxes[:, 0]
    y = boxes[:, 1]
    w = boxes[:, 2] - boxes[:, 0]
    h = boxes[:, 3] - boxes[:, 1]
 
    areas = w * h
    order = scores.argsort()[::-1]
 
    keep = []
    while order.size > 0:
        i = order[0]
        keep.append(i)
 
        xx1 = np.maximum(x[i], x[order[1:]])
        yy1 = np.maximum(y[i], y[order[1:]])
        xx2 = np.minimum(x[i] + w[i], x[order[1:]] + w[order[1:]])
        yy2 = np.minimum(y[i] + h[i], y[order[1:]] + h[order[1:]])
 
        w1 = np.maximum(0.0, xx2 - xx1 + 0.00001)
        h1 = np.maximum(0.0, yy2 - yy1 + 0.00001)
        inter = w1 * h1
 
        ovr = inter / (areas[i] + areas[order[1:]] - inter)
        inds = np.where(ovr <= NMS_THRESH)[0]
        order = order[inds + 1]
    keep = np.array(keep)
    return keep
 
 
def yolov5_post_process(input_data):
    masks = [[0, 1, 2], [3, 4, 5], [6, 7, 8]]
    anchors = [[10, 13], [16, 30], [33, 23], [30, 61], [62, 45],
               [59, 119], [116, 90], [156, 198], [373, 326]]
 
    boxes, classes, scores = [], [], []
    for input, mask in zip(input_data, masks):
        b, c, s = process(input, mask, anchors)
        b, c, s = filter_boxes(b, c, s)
        boxes.append(b)
        classes.append(c)
        scores.append(s)
 
    boxes = np.concatenate(boxes)
    boxes = xywh2xyxy(boxes)
    classes = np.concatenate(classes)
    scores = np.concatenate(scores)
 
    nboxes, nclasses, nscores = [], [], []
    for c in set(classes):
        inds = np.where(classes == c)
        b = boxes[inds]
        c = classes[inds]
        s = scores[inds]
 
        keep = nms_boxes(b, s)
 
        nboxes.append(b[keep])
        nclasses.append(c[keep])
        nscores.append(s[keep])
 
    if not nclasses and not nscores:
        return None, None, None
 
    boxes = np.concatenate(nboxes)
    classes = np.concatenate(nclasses)
    scores = np.concatenate(nscores)
 
    return boxes, classes, scores
 
 
def draw(image, boxes, scores, classes):
    """Draw the boxes on the image.
    # Argument:
        image: original image.
        boxes: ndarray, boxes of objects.
        classes: ndarray, classes of objects.
        scores: ndarray, scores of objects.
        all_classes: all classes name.
    """
    for box, score, cl in zip(boxes, scores, classes):
        top, left, right, bottom = box
        print('class: {}, score: {}'.format(CLASSES[cl], score))
        print('box coordinate left,top,right,down: [{}, {}, {}, {}]'.format(top, left, right, bottom))
        top = int(top)
        left = int(left)
        right = int(right)
        bottom = int(bottom)
 
        cv2.rectangle(image, (top, left), (right, bottom), (255, 0, 0), 2)
        cv2.putText(image, '{0} {1:.2f}'.format(CLASSES[cl], score),
                    (top, left - 6),
                    cv2.FONT_HERSHEY_SIMPLEX,
                    0.6, (0, 0, 255), 2)
 
 
def letterbox(im, new_shape=(640, 640), color=(0, 0, 0)):
    # Resize and pad image while meeting stride-multiple constraints
    shape = im.shape[:2]  # current shape [height, width]
    if isinstance(new_shape, int):
        new_shape = (new_shape, new_shape)
 
    # Scale ratio (new / old)
    r = min(new_shape[0] / shape[0], new_shape[1] / shape[1])
 
    # Compute padding
    ratio = r, r  # width, height ratios
    new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r))
    dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1]  # wh padding
 
    dw /= 2  # divide padding into 2 sides
    dh /= 2
 
    if shape[::-1] != new_unpad:  # resize
        im = cv2.resize(im, new_unpad, interpolation=cv2.INTER_LINEAR)
    top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
    left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
    im = cv2.copyMakeBorder(im, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color)  # add border
    return im, ratio, (dw, dh)
 
def detect_white_and_circles(image0, pub):
    # 转换图像到 HSV 色彩空间
    image = image0.copy()
    image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
 
    image = cv2.GaussianBlur(image, (5,5), 0)
    # 查找掩模图像中的圆形
    circles = cv2.HoughCircles(image, cv2.HOUGH_GRADIENT, 1, minDist=1000,param1=48, param2=57, minRadius=40, maxRadius=70)

    # 如果找到了圆形
    if circles is not None:
        circles = np.round(circles[0, :]).astype("int")  # 转换为整数
        
        for circle in circles:
            x, y, r = circle  # 提取圆形参数
            # 画圆形
            cv2.circle(image, (x, y), r, (0, 255, 255), 3)  # 绘制圆形边界
            cv2.circle(image, (x, y), 5, (0, 0, 255), 10)  # 绘制圆心
            # 在圆心旁边显示坐标和半径
            text = f"({x}, {y}), r={r}"
            cv2.putText(image, text, (x + 10, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2)
            
            # 发送圆心位置和半径信息
            msg = Yolo()
            msg.Class = str(2)
            msg.x = int(x)
            msg.y = int(y)
            msg.Confidence = 100
            pub.publish(msg)

    # 最终展示带有圆形标注的图像
    cv2.imshow("Detected Circles on White Areas", image)



if __name__ == '__main__':

    rknn = RKNNLite()

    ret = rknn.load_rknn(RKNN_MODEL)
 
 
    ret = rknn.init_runtime(core_mask=RKNNLite.NPU_CORE_0)  #使用0 1 2三个NPU核心
cap = cv2.VideoCapture('/dev/came1')
count  = 0


ledcount = 0
ledflag = 0
while not rospy.is_shutdown():

    ret, img = cap.read()
    if not ret:
        break
    count+=1
    ledcount+=1
    if ledcount >= 20:
        ledcount = 0
        ledflag = 1- ledflag
        if ledflag:

            os.system("gpio write 12 0")
        else:
            os.system("gpio write 12 1")




    detect_white_and_circles(img,pub)
    if count >= 1:
        count = 0

    
        img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        img = cv2.resize(img, (IMG_SIZE, IMG_SIZE))
        Real_img= np.expand_dims(img, axis=0)
        outputs = rknn.inference(inputs=[Real_img])
        input0_data = outputs[0]
        input1_data = outputs[1]
        input2_data = outputs[2]
    
        input0_data = input0_data.reshape([3, -1]+list(input0_data.shape[-2:]))
        input1_data = input1_data.reshape([3, -1]+list(input1_data.shape[-2:]))
        input2_data = input2_data.reshape([3, -1]+list(input2_data.shape[-2:]))
    
        input_data = list()
        input_data.append(np.transpose(input0_data, (2, 3, 0, 1)))
        input_data.append(np.transpose(input1_data, (2, 3, 0, 1)))
        input_data.append(np.transpose(input2_data, (2, 3, 0, 1)))
    
        boxes, classes, scores = yolov5_post_process(input_data)

        img_1 = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
        #findring(img_1)
        if boxes is not None:
            draw(img_1, boxes, scores, classes)
        
            for i, (cls, score) in enumerate(zip(classes, scores)):
                
                box = boxes[i]
                x_min, y_min, x_max, y_max = box

                # 计算中心坐标
                center_x = (x_min + x_max) / 2
                center_y = (y_min + y_max) / 2


                msg = Yolo()
                msg.Class = str(CLASSES[cls])
                msg.x = int(center_x)
                msg.y = int(center_y)
                msg.Confidence = score
                pub.publish(msg)

            
        
    
    #img_1 = cv2.resize(img_1, (320, 320))
        cv2.imshow("post process result", img_1)
    cv2.waitKey(1)
    rate.sleep()
 

cap.release()
 

cv2.destroyAllWindows()
rknn.release()


